package gnss

import (
	"dgo/goutils/utils"
	"fmt"
)

/*
**
RTCM

	同步码(8bit)    保留(6bit)        数据长度(10bit)   数据包           CRC(24 bits)

11010011     未定义，设为000000     以字节表示的数   0-1023 bytes    QualComm CRC-24Q

msm4, msm6 卫星数据 (18*NSat)
The number of integer milliseconds in GNSS Satellite rough ranges  DF397 uint8  (Nsat times) 8*Nsat
GNSS Satellite rough ranges modulo 1 millisecond        DF398 uint10 (Nsat times) 10*Nsat

Table 3.5-81 Content of Satellite Data for MSM5 and MSM7 (36*Nsat)
The number of integer milliseconds in GNSS Satellite rough ranges DF397 uint8 (Nsat times) 8*Nsat   //粗差
Extended Satellite Information Specific for each GNSS uint4 (Nsat times) 4*Nsat  Specific for each GNSS
GNSS Satellite rough ranges modulo 1 millisecond DF398 uint10 (Nsat times) 10*Nsat
GNSS Satellite rough PhaseRangeRates DF399 int14 (Nsat times) 14*Nsat   // 粗差相位速率

===========================================

Table 3.5-85 Content of Signal Data for MSM4 (48*Ncell)
GNSS signal fine Pseudoranges DF400 int15 (Ncell times) 15*Ncell
GNSS signal fine Phaserange data DF401 int22 (Ncell times) 22*Ncell
GNSS Phaserange Lock Time NaviIndicator DF402 uint4 (Ncell times) 4*Ncell
Half-cycle ambiguity indicator DF420 bit(1) (Ncell times) 1*Ncell
GNSS signal CNRs DF403 uint6 (Ncell times) 6*Ncell

Table 3.5-86 Content of Signal Data for MSM5 (63*Ncell)
GNSS signal fine Pseudoranges DF400 int15 (Ncell times) 15*Ncell
GNSS signal fine Phaserange data DF401 int22 (Ncell times) 22*Ncell
GNSS Phaserange Lock Time Indicator DF402 uint4 (Ncell times)4*Ncell
Half-cycle ambiguity indicator DF420 bit(1) (Ncell times) 1*Ncell
GNSS signal CNRs DF403 uint6 (Ncell times) 6*Ncell
GNSS signal fine PhaseRangeRates DF404 int15 (Ncell times) 15*Ncell

Table 3.5-88 Content of Signal Data for MSM7
GNSS signal fine Pseudoranges with extended resolution DF405 int20 (Ncell times) 20*Ncell
GNSS signal fine Phaserange data with extended resolution DF406 int24 (Ncell times) 24*Ncell
GNSS Phaserange Lock Time Indicator with extended range and resolution. DF407 uint10 (Ncell times) 10*Ncell
Half-cycle ambiguity indicator DF420 bit(1) (Ncell times) 1*Ncell
GNSS signal CNRs with extended resolution DF408 uint10 (Ncell times) 10*Ncell
GNSS signal fine PhaseRangeRates DF404 int15 (Ncell times) 15*Ncell
TOTAL 80*Ncell
*/
type RtcmRawMsm5 struct {
	Header RtcmRawMsmHeader

	// 卫星数据
	RoughRng1 [64]byte
	ExInfo    [64]byte // msm7
	RoughRng2 [64]uint16
	RoughPhRR [64]int16 // msm7  卫星数组

	// 信号数据
	PsRng   [64]int16 // 伪距
	PhRng   [64]int32 // 相位
	LkT     [64]uint16
	HalfAmb [64]byte
	CNR     [64]uint16
	PhaRR   [64]int16 // GNSS signal fine PhaseRangeRates DF404 int15 (Ncell times) 15*Ncell
}

func NewRtcmRawMsm5() *RtcmRawMsm5 {
	rval := &RtcmRawMsm5{}
	return rval
}

func (this *RtcmRawMsm5) GetHeader() *RtcmRawMsmHeader {
	return &this.Header
}

func (this *RtcmRawMsm5) DecodeHeader(buf []byte) (err error) {
	msgBuf := buf[3:]
	_, err = this.Header.Decode(msgBuf)
	return
}

func (this *RtcmRawMsm5) PsRngText(spliter string) string {
	var bb utils.BytesBuilder
	for i := U8_0; i < this.Header.CellN; i++ {
		if bb.Len() > 0 {
			bb.AppendStr(spliter)
		}
		bb.Appendf("%.4f", this.GetPsRngV(i))
	}
	return bb.String()
}

func (this *RtcmRawMsm5) PhRngText(spliter string) string {
	var bb utils.BytesBuilder
	for i := U8_0; i < this.Header.CellN; i++ {
		if bb.Len() > 0 {
			bb.AppendStr(spliter)
		}
		bb.Appendf("%.4f", this.GetPhRngV(i))
	}
	return bb.String()
}

func (this *RtcmRawMsm5) GetPsRngV(cellIdx byte) float64 {
	v := this.PsRng[cellIdx]
	if v == PS_INVALID_VALUE {
		return 0
	}
	return PsRng0To1_DF400(v)
}

func (this *RtcmRawMsm5) GetPhRngV(cellIdx byte) float64 {
	v := this.PhRng[cellIdx]
	if v == PH_INVALID_VALUE {
		return 0
	}
	return PhRng0To1_DF401(v)
}

func (this *RtcmRawMsm5) GetCnrV(cellIdx byte) float32 {
	v := float64(this.CNR[cellIdx])
	return float32(v)
}

/*
*

	OK
*/
func (this *RtcmRawMsm5) DecodeMsm(buf []byte) (err error) {
	defer func() { // 捕捉异常
		if e1 := recover(); e1 != nil {
			err = fmt.Errorf("解码MSM5数据出现异常:%v", e1)
			return
		}
	}()

	msgBuf := buf[3:]
	bitsN, e1 := this.Header.Decode(msgBuf)
	if e1 != nil {
		return err
	}

	//return;

	// ================================卫星数据
	/**
	The number of integer milliseconds in GNSS Satellite rough ranges DF397 uint8 (Nsat times) 8*Nsat   //粗差
	Extended Satellite Information Specific for each GNSS uint4 (Nsat times) 4*Nsat  Specific for each GNSS
	GNSS Satellite rough ranges modulo 1 millisecond DF398 uint10 (Nsat times) 10*Nsat
	GNSS Satellite rough PhaseRangeRates DF399 int14 (Nsat times) 14*Nsat   // 粗差相位速率
	*/
	//this.RoughRng1 = make([]byte, this.Header.SatN);
	for j := 0; j < int(this.Header.SatN); j++ {
		this.RoughRng1[j] = GetBitU8(msgBuf, bitsN, 8)
		bitsN += 8
	}

	for j := 0; j < int(this.Header.SatN); j++ {
		this.ExInfo[j] = GetBitU8(msgBuf, bitsN, 4)
		bitsN += 4
	}

	// GNSS 卫星低精度星地距离，以 1 毫秒为除数得到的1 毫秒内的部分
	//this.RoughRng2 = make([]uint16, this.Header.SatN);
	for j := 0; j < int(this.Header.SatN); j++ {
		v := GetBitU16(msgBuf, bitsN, 10)
		bitsN += 10
		this.RoughRng2[j] = v
	}

	for j := 0; j < int(this.Header.SatN); j++ {
		this.RoughPhRR[j] = GetBitI16(msgBuf, bitsN, 14)
		bitsN += 14
	}

	// ========================================信号数据
	/**
	Table 3.5-86 Content of Signal Data for MSM5 (63*Ncell)
	GNSS signal fine Pseudoranges DF400 int15 (Ncell times) 15*Ncell
	GNSS signal fine Phaserange data DF401 int22 (Ncell times) 22*Ncell
	GNSS Phaserange Lock Time Indicator DF402 uint4 (Ncell times)4*Ncell
	Half-cycle ambiguity indicator DF420 bit(1) (Ncell times) 1*Ncell
	GNSS signal CNRs DF403 uint6 (Ncell times) 6*Ncell
	GNSS signal fine PhaseRangeRates DF404 int15 (Ncell times) 15*Ncell
	*/
	//// GNSS 信号高精度伪距（fine Psedoranges）   DF405
	//this.PsRng = make([]int16, this.Header.CellN)
	for j := 0; j < int(this.Header.CellN); j++ { //  pseudorange
		v := GetBitI16(msgBuf, bitsN, 15)
		bitsN += 15
		this.PsRng[j] = v
	}

	//// GNSS 信号高精度载波相位（fine PhaseRange） DF406    phaserange
	//this.PhRng = make([]int32, this.Header.CellN)
	for j := 0; j < int(this.Header.CellN); j++ {
		v := GetBitI32(msgBuf, bitsN, 22)
		bitsN += 22
		this.PhRng[j] = v
	}

	//// lock time
	//this.LkT = make([]byte, this.Header.CellN)
	for j := 0; j < int(this.Header.CellN); j++ {
		v := GetBitU16(msgBuf, bitsN, 4)
		bitsN += 4
		this.LkT[j] = v
	}

	// half-cycle ambiguity
	for j := 0; j < int(this.Header.CellN); j++ {
		v := GetBitU8(msgBuf, bitsN, 1)
		bitsN += 1
		this.HalfAmb[j] = v
	}

	// CNR
	for j := 0; j < int(this.Header.CellN); j++ {
		v := GetBitU16(msgBuf, bitsN, 6)
		bitsN += 6
		this.CNR[j] = v
	}

	// DF404
	for j := 0; j < int(this.Header.CellN); j++ { //
		v := GetBitI16(msgBuf, bitsN, 15)
		bitsN += 15
		this.PhaRR[j] = v
	}

	return nil
}

/*
*

	未完成
*/
func (this *RtcmRawMsm5) EncodeMsm(bb *utils.BytesBuilder, startIdx int) (encodeBitN int) {
	bb.CheckSize(startIdx, 3)
	bb.Replace(startIdx, []byte{0xD3, 0, 0})
	startIdx0 := startIdx
	startIdx += 3
	encodeBitN = this.Header.Encode(bb, startIdx)
	satN := int(this.Header.SatN)
	cellN := int(this.Header.CellN)
	bitTotal := (encodeBitN + 36*int(this.Header.SatN) + 80*int(this.Header.CellN))
	ByteN := bitTotal / 8
	if bitTotal%8 > 0 {
		ByteN++
	}
	bb.CheckSize(startIdx, ByteN+3) // 3:CRC

	/// Satellite
	for i := 0; i < satN; i++ {
		bb.SetBitU8(startIdx, encodeBitN, 8, this.RoughRng1[i])
		encodeBitN += 8
	}

	for i := 0; i < satN; i++ {
		bb.SetBitU8(startIdx, encodeBitN, 4, this.ExInfo[i])
		encodeBitN += 4
	}

	for i := 0; i < satN; i++ {
		bb.SetBitU16(startIdx, encodeBitN, 10, this.RoughRng2[i])
		encodeBitN += 10
	}

	for i := 0; i < satN; i++ {
		bb.SetBitI16(startIdx, encodeBitN, 14, this.RoughPhRR[i])
		encodeBitN += 14
	}

	// Signal
	for i := 0; i < cellN; i++ {
		bb.SetBitI16(startIdx, encodeBitN, 15, this.PsRng[i])
		encodeBitN += 20
	}

	for i := 0; i < cellN; i++ {
		bb.SetBitI32(startIdx, encodeBitN, 24, this.PhRng[i])
		encodeBitN += 24
	}

	for i := 0; i < cellN; i++ {
		bb.SetBitU16(startIdx, encodeBitN, 10, this.LkT[i])
		encodeBitN += 10
	}

	for i := 0; i < cellN; i++ {
		bb.SetBitU8(startIdx, encodeBitN, 1, this.HalfAmb[i])
		encodeBitN += 1
	}

	for i := 0; i < cellN; i++ {
		bb.SetBitU16(startIdx, encodeBitN, 10, this.CNR[i])
		encodeBitN += 10
	}

	for i := 0; i < cellN; i++ {
		bb.SetBitI16(startIdx, encodeBitN, 15, this.PhaRR[i])
		encodeBitN += 15
	}

	// 长度
	bb.SetBitU16(startIdx0, 8+6, 10, uint16(ByteN))
	crcBuf := bb.Buff(startIdx0, ByteN+3) // 3:同步标记 + 保留位 + 长度（RTCM消息体）(10bit)
	crc1 := utils.CRC24QBuf(crcBuf)
	bb.SetBitU32(startIdx0+3+ByteN, 0, 24, uint32(crc1))
	bb.Delete2End(startIdx0 + 3 + ByteN + 3)
	return
}

/*
**

	msm4, msm5, 7都可以用
*/
func (this *RtcmRawMsm5) GetRoughR(satIdx byte) float64 {
	rng1 := this.RoughRng1[satIdx]
	v := float64(0)
	if rng1 != 255 {
		v = float64(rng1) * RANGE_MS
	}

	rng2 := this.RoughRng2[satIdx]
	v += float64(rng2) * P2_10 * RANGE_MS
	return v
}

/*
**

	msm4, msm5, 7都可以用
*/
func (this *RtcmRawMsm5) SetRoughR(satIdx byte, v float64) {
	r1 := RoundU(v / RANGE_MS / P2_10)

	i_ms := uint(r1 >> 10)
	this.RoughRng1[satIdx] = byte(i_ms)

	mod_ms := uint(r1 & 0x3FF)
	this.RoughRng2[satIdx] = uint16(mod_ms)
}

func (this *RtcmRawMsm5) RangeObs(cb func(sigobs *RtcmRawSatSigObs)) {

	sats := this.Header.ExtractSats()
	sigs := this.Header.ExtractSigs()

	n := this.Header.SatN * this.Header.SigN
	u64 := this.Header.CellMask
	j := U8_0
	var sigobs RtcmRawSatSigObs
	sigobs.NaviSys = this.Header.NaviSys

	sigidx := U8_0
	cellIdx := U8_0

	for i := U8_0; i < n; i++ { // 循环 卫星x信号 排列数据
		if byte(i)%this.Header.SigN == 0 {
			sigobs.SatId = sats[j]
			sigobs.RoughRR = this.GetRoughR(j)
			sigobs.RoughPRR = this.RoughPhRR[j]
			j++
			sigidx = 0
		}
		mask := (u64 >> (n - 1 - i)) & 1
		if mask == 1 {
			sigV := sigs[sigidx] // 信号ID
			sigobs.SigId = sigV
			sigobs.PsRng0 = this.GetPsRngV(cellIdx)
			sigobs.PhRng0 = this.GetPhRngV(cellIdx)

			sigobs.LktV = uint16(this.LkT[cellIdx])
			sigobs.HalfAmb = this.HalfAmb[cellIdx]
			sigobs.Cnr = this.GetCnrV(cellIdx)
			sigobs.PhaRR = float64(this.PhaRR[cellIdx]) * 0.0001
			cb(&sigobs)
			cellIdx++
		}
		sigidx++
	}
}

func (this *RtcmRawMsm5) WriteToObs(msmObs *RtcmRawMsmObs) {

	sats := this.Header.ExtractSats()
	sigs := this.Header.ExtractSigs()

	n := this.Header.SatN * this.Header.SigN
	u64 := this.Header.CellMask
	j := U8_0
	var satObs *RtcmRawObsSat = nil
	sigidx := U8_0
	cellIdx := U8_0
	for i := U8_0; i < n; i++ {
		if byte(i)%this.Header.SigN == 0 {
			satObs = msmObs.CheckGetSatObs(sats[j])
			satObs.Satid = sats[j]
			satObs.RoughRR = this.GetRoughR(j)
			j++
			sigidx = 0
		}
		mask := (u64 >> (n - 1 - i)) & 1
		if mask == 1 {
			sigV := sigs[sigidx] - 1 // 信号ID
			satObs.PsRngV[sigV] = this.GetPsRngV(cellIdx)
			satObs.PhRngV[sigV] = this.GetPhRngV(cellIdx)
			satObs.LkTV[sigV] = uint16(this.LkT[cellIdx])
			satObs.HalfAmb[sigV] = this.HalfAmb[cellIdx]
			satObs.Cnr[sigV] = this.GetCnrV(cellIdx)
			cellIdx++
		}
		sigidx++
	}
}
